#include "mytask.h" 
#include "io.h"
#include "queue.h"
#include "FreeRTOS.h"
#include "oled.h"
#include "bmp.h"
#include "time.h"
#include "math.h"
#include "mpu-6050.h"
#include "imu.h"
#include "usart.h"
#include "string.h"
#include "mymath.h"
#include "filter.h"
#include "3D_model.h"
#include "pwm.h"
 #include "adc.h"
extern	 float rol;
extern	 float pit;
extern	 float yaw;	
struct myaxis g;
struct myaxis a;
struct myaxis g_d;
struct myaxis a_d;
extern u16 RX_CH[];

u8 menu=0;
QueueHandle_t Key_Queue;   		//按键值消息队列句柄
QueueHandle_t Message_Queue;	//信息队列句柄
#define KEYMSG_Q_NUM    2  		//按键消息队列的数量  
//任务优先级
#define START_TASK_PRIO		1
//任务堆栈大小	
#define START_STK_SIZE 		256  
//任务句柄
TaskHandle_t StartTask_Handler;
//任务函数
void start_task(void *pvParameters);


//任务句柄
TaskHandle_t OLED_Task_Handler;
TaskHandle_t OLED_creat_Handler;
TaskHandle_t ppm_display_Handler;
//任务函数
void key_task(void *pvParameters);
void air_task(void *pvParameters);


void task_init(void)
{
	
	 //创建开始任务
    xTaskCreate((TaskFunction_t )start_task,            //任务函数
                (const char*    )"start_task",          //任务名称
                (uint16_t       )START_STK_SIZE,        //任务堆栈大小
                (void*          )NULL,                  //传递给任务函数的参数
                (UBaseType_t    )START_TASK_PRIO,       //任务优先级
                (TaskHandle_t*  )&StartTask_Handler);   //任务句柄              
    vTaskStartScheduler();          //开启任务调度
}

void start_task(void *pvParameters)
{

    taskENTER_CRITICAL();           //进入临界区               
   xTaskCreate((TaskFunction_t )key_task,     	
               (const char*    )"key_task",   	
               (uint16_t       )256, 
               (void*          )NULL,				
               (UBaseType_t    )10,	
               (TaskHandle_t*  )NULL); 
		xTaskCreate((TaskFunction_t )air_task,     	
               (const char*    )"air_task",   	
               (uint16_t       )256, 
               (void*          )NULL,				
               (UBaseType_t    )10,	
               (TaskHandle_t*  )NULL); 
		// xTaskCreate((TaskFunction_t )ppm_display,     	
        //         (const char*    )"ppm_display",   	
        //         (uint16_t       )256, 
        //         (void*          )NULL,				
        //         (UBaseType_t    )9,	
        //         (TaskHandle_t*  )&ppm_display_Handler); 
													
								
	 taskEXIT_CRITICAL();            //退出临界区							
								
    vTaskDelete(StartTask_Handler); //删除开始任务
   
}



	

void ppm_display (void)
{

while(menu==1)
	{
		for(u8 i=0;i<8;i++)
		{
		 LCD_DrawRectangle(8+i*15,5,14+i*15,55);
		 OLED_ShowNum(8+i*15,57,i+1,1,12);
			if(RX_CH[i]<1000)RX_CH[i]=1000;
			if(RX_CH[i]>2000)RX_CH[i]=2000;
		 dispaly_fill_rect(8+i*15,55-(RX_CH[i]-1000)/20,14+i*15,55,1);
		}
		//OLED_ShowNum(0,0,RX_CH[0],4,12);
		//OLED_ShowNum(0,50,1000000/(u32)Get_Cycle_T(1),3,12);
		display();
		dispaly_Clear();
		
		 //vTaskDelay(50);
	}
}

void key_task(void *pvParameters)
{
	  u8 key_Flag=0;
    while(1)
    {
        if((KEY_Scan()==1)&&(menu<3)&&(key_Flag==0))
				{
					 key_Flag++;
           menu++;
				}
				else if ((KEY_Scan()==0)&&(key_Flag==1))
					key_Flag=0;
        if(menu==3)
          menu=0;

        vTaskDelay(50);
    }
  
        
}

void air_task(void *pvParameters)
{

	    while(1)
    {
       switch (menu)
       {
        case 0:
				  	oc_stop();
					  ppm_out_start();	
            instrument_display();

            break;

        case 1:	
	          ppm_out_stop();
						oc_start();
            ppm_display();
            break;
			  case 2:
					OLED_ShowNum(0,10, Get_Adc(8),4,12);
				OLED_ShowNum(0,20, Get_Adc(9),4,12);
					 display();
						dispaly_Clear();
						vTaskDelay(50);
						//oc_stop();
						//ppm_out_start();
           // ppm_display();
            break;

        default:
            break;
       }

       
    }

        
}






 void OLED_task(void *pvParameters)
{
	float cube[8][3]={{-2,0,0},{5,0,0},{10,-5,0},{15,0,0},{22,0,0}};//模型各点坐标
	float cube_bb[8][3]={{-2,0,0},{5,0,0},{10,-5,0},{15,0,0},{22,0,0}};//模型上一时间节点各点坐标
	int lineid[]={1,2,2,3,3,4,4,5};//模型点之间连接顺序
		for(int i=0;i<4;i++)
		  for(int j=0;j<3;j++)
					{
					   cube[i][j]=cube[i][j]*3;
					   cube_bb[i][j]=cube_bb[i][j]*3;
					}

		      
	
		vTaskDelay(500);
		MPU_Get_Gyroscope(&g_d.x,&g_d.y,&g_d.z);                
		MPU_Get_Accelerometer(&a_d.x,&a_d.y,&a_d.z);
      vTaskDelay(500);
	while(1)
	{
		MPU_Get_Gyroscope(&g.x,&g.y,&g.z);                
		MPU_Get_Accelerometer(&a.x,&a.y,&a.z);
		IMU_update((float)Get_Cycle_T(0)/1000000.0f);
	//	dispaly_Clear();

		for(int i=0;i<4;i++)rotate(cube[i],rol,pit,0);//旋转每个点
		
		for(int i=0;i<12;i+=2){//绘制立方体
			LCD_DrawLine(64+cube[lineid[i]-1][0],40+cube[lineid[i]-1][1],64+cube[lineid[i+1]-1][0],40+cube[lineid[i+1]-1][1]);
		
		}
		
     memcpy(cube,cube_bb,sizeof(cube));
     OLED_ShowNum(0,6,1000000/(u32)Get_Cycle_T(1),3,12);
		 display(); 
     		
	// delay_ms(16);	
}
} 


